//
// Created by l on 2021/11/8.
//

#ifndef UNTITLED_ASTAR_RIGHT_H
#define UNTITLED_ASTAR_RIGHT_H
#ifndef ASTAR_H
#define ASTAR_H

#include <sys/time.h>
#include <iostream>
#include <queue>
#include <vector>
#include <stack>
#include <algorithm>
#include "Point.h"

using namespace std;


class Astar {
public:

    enum RMapItemType {
        RMAP_FREE = 0,                    // 空白区域 白色
        RMAP_OBSTACLE_UNDESCOVERED = 1,        // 未被发现的障碍物 深灰色
        RMAP_OBSTACLE_DILATE = 2,        // 膨胀的障碍物 浅灰色
        RMAP_OBSTACLE,                    // 障碍物
        RMAP_OBSTACLE_SCANNED,            // 已被扫描到的障碍物 黑色
        RMAP_START_POINT,                // 起点
        RMAP_END_POINT,                    // 终点
        RMAP_CURRENT_POSITION,            // 当前点
        RMAP_OBSTACLE_EDGE                // 障碍物扩展的点
    };


    Astar(int x, int y) {
        this->row = x;
        this->col = y;
    }


    void search(point *startPos, point *endPos);

    void checkPoit(int x, int y, point *parent, int g);

    void NextStep(point *currentPoint);

    static int isContains(vector<point *> *Pointlist, int x, int y);

    static void countGHF(point *sPoint, point *ePoint, int g);

    static bool compare(point *n1, point *n2);

    bool unWalk(int x, int y) const;

    void printPath(point *current);


    //priority_queue<point *, vector<point *>, less<int> > openList;
    vector<point *> openList;
    vector<point *> closeList;
    point *startPos;
    point *endPos;
    int8_t **map;
    vector<point> path;
    static const int WeightW = 10;// 正方向消耗
    static const int WeightWH = 14;//打斜方向的消耗
    int row = 100;
    int col = 100;
};


void Astar::search(point *startPos, point *endPos) {

    if (startPos->x < 0 || startPos->x >= row || startPos->y < 0 || startPos->y >= col
        ||
        endPos->x < 0 || endPos->x >= row || endPos->y < 0 || endPos->y >= col)
        return;
    point *current;
    this->startPos = startPos;
    this->endPos = endPos;
    openList.push_back(startPos);
    //主要是这块，把开始的节点放入openlist后开始查找旁边的8个节点，如果坐标超长范围或在closelist就return 如果已经存在openlist就对比当前节点到遍历到的那个节点的G值和当前节点到原来父节点的G值 如果原来的G值比较大 不用管 否则重新赋值G值 父节点 和f 如果是新节点 加入到openlist直到opellist为空或找到终点
    while (!openList.empty()) {
        current = openList[0];
        if (current->x == endPos->x && current->y == endPos->y) {
            cout << "find the path?" << endl;
            //printMap();

            printPath(current);
            for (auto &p: path) {
                cout << p.getx() << " " << p.gety() << endl;
            }

            openList.clear();
            closeList.clear();
            break;
        }
        NextStep(current);
        closeList.push_back(current);
        openList.erase(openList.begin());
        //struct timeval t1, t2;
        //gettimeofday(&t1, NULL);
        //sort(openList.begin(), openList.end(), compare);
        //cout << (t2.tv_sec - t1.tv_sec) * 1000000 + t2.tv_usec - t1.tv_usec << "us" << endl;
    }
}

void Astar::checkPoit(int x, int y, point *parent, int g) {
    if (x < 0 || x >= row || y < 0 || y >= col)
        return;
    if (this->unWalk(x, y))
        return;
    if (isContains(&closeList, x, y) != -1)
        return;
    int index;
    if ((index = isContains(&openList, x, y)) != -1) {
        point *point = openList[index];
        if (point->G > parent->G + g) {
            point->parent = parent;
            point->G = parent->G + g;
            point->F = point->G + point->H;
        }
    } else {
        auto *point = new class point(x, y, parent);
        countGHF(point, endPos, g);
        openList.push_back(point);
    }
}

void Astar::NextStep(point *current) {
    checkPoit(current->x - 1, current->y, current, WeightW);//左
    checkPoit(current->x + 1, current->y, current, WeightW);//右
    checkPoit(current->x, current->y + 1, current, WeightW);//上
    checkPoit(current->x, current->y - 1, current, WeightW);//下
    checkPoit(current->x - 1, current->y + 1, current, WeightWH);//左上
    checkPoit(current->x - 1, current->y - 1, current, WeightWH);//左下
    checkPoit(current->x + 1, current->y - 1, current, WeightWH);//右下
    checkPoit(current->x + 1, current->y + 1, current, WeightWH);//右上
}

int Astar::isContains(vector<point *> *Pointlist, int x, int y) {
    for (int i = 0; i < Pointlist->size(); i++) {
        if (Pointlist->at(i)->x == x && Pointlist->at(i)->y == y) {
            return i;
        }
    }
    return -1;
}

void Astar::countGHF(point *sPoint, point *ePoint, int g) {
    int h = abs(sPoint->x - ePoint->x) * WeightW + abs(sPoint->y - ePoint->y) * WeightW;
    int currentg = sPoint->parent->G + g;
    int f = currentg + h;
    sPoint->G = f;
    sPoint->H = h;
    sPoint->G = currentg;
}

bool Astar::compare(point *n1, point *n2) {
    //printf("%d,%d",n1->f,n2->f);
    return n1->F < n2->F;
}

bool Astar::unWalk(int x, int y) const {
    //cout << x << " " << y << endl;
    if (map[x][y] == RMAP_OBSTACLE_SCANNED )//|| map[x][y] == RMAP_OBSTACLE_UNDESCOVERED)
        return true;
    return false;
}

void Astar::printPath(point *current) {
    if (current->parent != nullptr)
        printPath(current->parent);
    //map[current->x][current->y] = 6;
    path.emplace_back(current->x, current->y);
    //printf("(%d,%d)", current->x, current->y);
}


#endif
#endif //UNTITLED_ASTAR_RIGHT_H
